#include "peripheral_init.h"
#include <stdarg.h>

/**
 * This function will return the length of a string, which terminate will
 * null character.
 *
 * @param s the string
 *
 * @return the length of string
 */
uint32_t rt_strlen(const char *s)
{
	const char *sc;
	for (sc = s; *sc != '\0'; ++sc) /* nothing */
		;
	return sc - s;
}
#define isdigit(c)  ((unsigned)((c) - '0') < 10)
//static inline 
int32_t divide(int32_t *n, int32_t base)
{
	int32_t res;

	/* optimized for processor which does not support divide instructions. */
	if (base == 10)
	{
		res = ((uint32_t)*n) % 10U;
		*n = ((uint32_t)*n) / 10U;
	}
	else
	{
		res = ((uint32_t)*n) % 16U;
		*n = ((uint32_t)*n) / 16U;
	}

	return res;
}
//static inline int skip_atoi(const char **s)
static  int skip_atoi(const char **s)
{
	register int i=0;
	while (isdigit(**s)) i = i*10 + *((*s)++) - '0';
	return i;
}
#define ZEROPAD 	(1 << 0)	/* pad with zero */
#define SIGN 		(1 << 1)	/* unsigned/signed long */
#define PLUS 		(1 << 2)	/* show plus */
#define SPACE 		(1 << 3)	/* space if plus */
#define LEFT 		(1 << 4)	/* left justified */
#define SPECIAL 	(1 << 5)	/* 0x */
#define LARGE		(1 << 6)	/* use 'ABCDEF' instead of 'abcdef' */

/* private function */

static unsigned char *print_number(unsigned char * buf, unsigned char * end, long num, int base, int s, int type)
{
  char c, sign;
  char tmp[16];
  const char *digits;
  static const char small_digits[] = "0123456789abcdef";
  static const char large_digits[] = "0123456789ABCDEF";
  register int i;
  register int size;
  
  size = s;
  
  digits = (type & LARGE) ? large_digits : small_digits;
  if (type & LEFT) type &= ~ZEROPAD;
  
  c = (type & ZEROPAD) ? '0' : ' ';
  
  /* get sign */
  sign = 0;
  if (type & SIGN)
  {
    if (num < 0)
    {
      sign = '-';
      num = -num;
    }
    else if (type & PLUS) sign = '+';
    else if (type & SPACE) sign = ' ';
  }
  i = 0;
  if (num == 0) tmp[i++]='0';
  else
  {
    while (num != 0) tmp[i++] = digits[divide((int32_t *)&num, base)];
  }
  size -= i;
  if (!(type&(ZEROPAD | LEFT)))
  {
    while(size-->0)
    {
      if (buf <= end) *buf = ' ';
      ++buf;
    }
  }
  
  if (sign)
  {
    if (buf <= end)
    {
      *buf = sign;
      --size;
    }
    ++buf;
  }
  /* no align to the left */
  if (!(type & LEFT))
  {
    while (size-- > 0)
    {
      if (buf <= end) *buf = c;
      ++buf;
    }
  }
  
  /* put number in the temporary buffer */
  while (i-- > 0)
  {
    if (buf <= end) *buf = tmp[i];
    ++buf;
  }
  
  while (size-- > 0)
  {
    if (buf <= end) *buf = ' ';
    ++buf;
  }
  
  return buf;
}

void nprintf(unsigned char *buf, uint32_t size, const char *fmt, va_list args)
{
  int32_t num;
  //	int i, len;
  unsigned char *str, *end;//, c;
  //	const char *s;
  
  uint8_t base;			/* the base of number */
  uint8_t flags;			/* flags to print number */
  uint8_t qualifier;		/* 'h', 'l', or 'L' for integer fields */
  int32_t field_width;	/* width of output field */
  
  str = buf;
  end = buf + size - 1;
  /* Make sure end is always >= buf */
  if (end < buf)
  {
    end = ((unsigned char *)-1);
    size = end - buf;
  }
  
  for (; *fmt ; ++fmt)
  {
    if (*fmt != '%')
    {
      if (str <= end) *str = *fmt;
      ++str;
      continue;
    }
    /* process flags */
    flags = 0;
    while(1)
    {
      /* skips the first '%' also */
      ++fmt;
      if (*fmt == '-') flags |= LEFT;
      else if (*fmt == '+') flags |= PLUS;
      else if (*fmt == ' ') flags |= SPACE;
      else if (*fmt == '#') flags |= SPECIAL;
      else if (*fmt == '0') flags |= ZEROPAD;
      else break;
    }
    
    /* get field width */
    field_width = -1;
    if (isdigit(*fmt)) field_width = skip_atoi(&fmt);
    else if (*fmt == '*')
    {
      ++fmt;
      /* it's the next argument */
      field_width = va_arg(args, int);
      if (field_width < 0)
      {
        field_width = -field_width;
        flags |= LEFT;
      }
    }
    /* get the conversion qualifier */
    qualifier = 0;
    if (*fmt == 'h' || *fmt == 'l')
    {
      qualifier = *fmt;
      ++fmt;
    }
    /* the default base */
    base = 10;
    switch (*fmt)
    {      
    case '%':
      if (str <= end) *str = '%';
      ++str;
      continue;
      /* integer number formats - set up the flags and "break" */
    case 'o':
      base = 8;
      break;
      
    case 'X':
      flags |= LARGE;
    case 'x':
      base = 16;
      break;
      
    case 'd':
    case 'i':
      flags |= SIGN;
    case 'u':
      break;
      
    default:
      if (str <= end) *str = '%';
      ++str;
      if (*fmt)
      {
        if (str <= end) *str = *fmt;
        ++str;
      }
      else
      {
        --fmt;
      }
      continue;
    }
    if (qualifier == 'h')
    {
      num = (uint16_t) va_arg(args, int32_t);
      if (flags & SIGN) num = (int16_t) num;
    }
    else
    {
      num = va_arg(args, uint32_t);
      if (flags & SIGN) num = (int32_t) num;
    }
    str = print_number(str, end, num, base, field_width, flags);
  }
  if (str <= end) *str = '\0';
  else 
  *end = '\0';
  
}

/**
 * This function will fill a formatted string to buffer
 *
 * @param buf the buffer to save formatted string
 * @param format the format
 */
void rt_sprintf(unsigned char *buf ,const char *format,...)
{
  va_list arg_ptr;
  va_start(arg_ptr, format);
  nprintf(buf, (uint32_t) -1, format, arg_ptr);
  va_end (arg_ptr);
}
//
